//
// Created by PulsarV on 18-10-25.
//

#include <rc_mapping/slam_devices.h>
#include <rplidar.h>
#include <GL/glut.h>
#include <rplidar_driver.h>
#include <unistd.h>
#include <iostream>
#include <math.h>
#include <base/slog.hpp>
#include <stdio.h>

#define RADAR_STEP 0.01
#define PI 3.1415926535
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))


namespace RC {
    namespace Map {
        using namespace rp::standalone::rplidar;

        bool checkRPLIDARHealth(RPlidarDriver *drv) {
            u_result op_result;
            rplidar_response_device_health_t healthinfo;


            op_result = drv->getHealth(healthinfo);
            if (IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed.
                printf("RPLidar health status : %d\n", healthinfo.status);
                if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
                    fprintf(stderr, "Error, rplidar internal error detected. Please reboot the device to retry.\n");
                    return false;
                } else {
                    return true;
                }

            } else {
                fprintf(stderr, "Error, cannot retrieve the lidar health code: %x\n", op_result);
                return false;
            }
        }

        SlamDevice::SlamDevice(const std::string &device_path) {
            slog::info<<device_path<<slog::endl;
            rplidar_response_device_info_t devinfo;
            const char *opt_com_path = NULL;
            _u32 baudrateArray[2] = {115200, 256000};
            u_result op_result;

            rPlidarDriver = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);

            size_t baudRateArraySize = (sizeof(baudrateArray)) / (sizeof(baudrateArray[0]));

            for (size_t i = 0; i < baudRateArraySize; ++i) {
                if (!rPlidarDriver)
                    rPlidarDriver = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
                if (IS_OK(rPlidarDriver->connect(device_path.c_str(), baudrateArray[i]))) {
                    op_result = rPlidarDriver->getDeviceInfo(devinfo);

                    if (IS_OK(op_result)) {
                        connectSuccess = true;
                        break;
                    }
                }
            }
            if (!connectSuccess) {
                slog::err << "Error, cannot bind to the specified serial port " << device_path << slog::endl;
            }
            slog::info << "RPLIDAR S/N: ";
            for (int pos = 0; pos < 16; ++pos) {
                printf("%02X", devinfo.serialnum[pos]);
            }
            slog::info << slog::endl;
            slog::info << "RPLIDAR Version ";
            printf("\n"
                   "Firmware Ver: %d.%02d\n"
                   "Hardware Rev: %d\n", devinfo.firmware_version >> 8, devinfo.firmware_version & 0xFF,
                   (int) devinfo.hardware_version);
            slog::info << slog::endl;
            if (!checkRPLIDARHealth(rPlidarDriver)) {
                slog::err << "Error, radar health is false " << slog::endl;
            }
        }

        void SlamDevice::bind(void (*call_function)(std::vector <DOT>)) {
            this->call_function=call_function;
        }

        void SlamDevice::start_motor() {
            rPlidarDriver->startMotor();
        }

        void SlamDevice::start_scan(bool force, _u32 timeout) {
            rPlidarDriver->startScan(force, timeout);
        }

        void SlamDevice::stop() {
            rPlidarDriver->stop();
            rPlidarDriver->stopMotor();
        }

        void SlamDevice::release() {
            RPlidarDriver::DisposeDriver(rPlidarDriver);
            rPlidarDriver = nullptr;
        }

        void SlamDevice::recive() {
            if(this->is_start){
                std::vector <DOT> dots;
                float p1_x = 0, p1_y = 0;
                float p2_x = 0, p2_y = 0;
                float p1step = 1.75f;
                float p2step = 2.25f;

                p1_x = sin(PI * p1step);
                p1_y = cos(PI * p1step);
                p2_x = sin(PI * p2step);
                p2_y = cos(PI * p2step);
                if (p1step == 2.5 + 1.75)
                    p1step = 1.75;
                if (p2step == 2.5 + 2.25)
                    p2step = 2.25;
                p1step += RADAR_STEP;
                p2step += RADAR_STEP;
                rplidar_response_measurement_node_t nodes[360 * 22];
                size_t count = _countof(nodes);
                u_result op_result;
                op_result = rPlidarDriver->grabScanData(nodes, count);
                if (IS_OK(op_result)) {
                    rPlidarDriver->ascendScanData(nodes, count);
                    dots.clear();
                    for (int pos = 0; pos < (int) count; ++pos) {
                        DOT dot(((nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f),
                                (nodes[pos].distance_q2 / 4.0f));
                        dots.push_back(dot);
//                        printf("%s theta: %f Dist: %08.2f Q: %d \n",
//                               (nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ? "S " : "  ",
//                               (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f,
//                               nodes[pos].distance_q2 / 4.0f,
//                               nodes[pos].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
                    }
                    this->call_function(dots);
                }
            }
        }

        void SlamDevice::start(bool force, _u32 timeout) {
            if(this->connectSuccess){
                rPlidarDriver->startScan(force, timeout);
                rPlidarDriver->startMotor();
                this->is_start=true;
            }
        }
    }
}
